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ABSTRACT 

We consider the estimation of the post-failure workspace of 
two two-link serial robots where the free-swinging failure 
of one robot’s last joint is handled by having the functional 
robot grasp the final link of the broken robot. We present 
an algorithm for finding the optimal placement of such syn- 
ergistic robot arms and the optimal grasp point on the final 
link of the broken robot. To determine whether a point in 
the pre-failure workspace of the broken robot remains in 
the post-failure workspace, we solve an inverse kinemat- 
ics problem in the form of a polynomial system. Homo- 
topy continuation provides an efficient means for solving 
such polynomial systems, so we use the software package 
Bertini. Finally, Monte-Carlo methods are used to estimate 
the post-failure workspace. 


KEY WORDS 
Kinematic redundancy, robot arms, workspace, joint fail- 
ure, homotopy continuation, Monte Carlo methods. 


1. Introduction 


Fail-proofing and robustness play an important role in the 
design of autonomous systems, including robotic arms. 
While no robot can be completely fail-proof, the probabil- 
ity of several joints failing simultaneously is low. In this 
paper, we consider autonomously operated robotic systems 
that are deployed into a hazardous or remote location, such 
that the repair on a failed joint is impractical or impossible. 
Suppose the system has to operate for an extended period 
of time, and we want the system to operate to the best of its 
remaining physical capability after a joint failure. 

Suppose such a system possesses several robotic arms 
for various tasks. Should one arm fail, the presence of 
a second arm may open the way to preserve some of the 
workspace of the failed arm. In particular, if the second 
arm can grasp the first, perhaps some of the lost workspace 
of the first arm can be restored. While, in principle, these 
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arms should operate in three dimensions, here, we sim- 
plify our considerations for the case of planar arms for the 
sake of simplicity. Our methods are directly transferrable 
to three dimensions without substantial alteration. Thus, 
in this paper, we consider two two-dimensional two-linked 
rotary robots working sufficiently close to each other, such 
that their workspaces overlap. Then, if a joint on one robot 
fails, the other grasps a given point on the broken robot, 
thus restoring some of the lost workspace. 


Immediately, two questions arise: a) What is the best 
placement of the robots in relation to each other? b) Is 
there a best place for the second arm to grasp the first? 
We should mention here that the answer to both ques- 
tions clearly depends on the measure of the post-failure 
workspace, which is clearly problem dependent. In this 
paper, we use the standard Euclidian measure to quantify 
the size of the workspace. A choice of other measures (for 
example, closeness to a target) will change the details of 
the optimization procedure, but the basic structure of the 
solution process should remain unchanged. These gener- 
alizations lie beyond the scope of this paper. Our focus in 
this paper is to find the optimal separation and grasp point 
for a two-jointed robot to grasp another two-jointed robot 
in the event of a free-swinging joint failure, so as to max- 
imize the area of the post-failure workspace. To solve this 
problem, we use the method of homotopy continuation. In 
particular, the inverse kinematics problem can be cast as a 
polynomial system, and homotopy continuation provides a 
means for efficiently producing numerical approximations 
of all solutions of the polynomial system. 


We recognize that, for this particular case, the prob- 
lem could be solved in almost closed form. However, this 
traditional approach fails to generalize when additional de- 
grees of freedom are present, e.g. higher dimensions or 
more links, whereas our approach is directly transferrable 
to higher dimensions. 


Consideration of fault-tolerance is essential in dan- 
gerous locations, and has been studied extensively, as in 
[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17], 
and the references therein. Methods of algebraic geometry 
have been applied in the kinematic setting as well. See for 
example [18, 19, 20, 21]. However, analysis of the coop- 


eration of multiple arms via the tools of numeric algebraic 
geometry has never been explored, making the approach of 
this article significantly different from all related literature. 

In the next section, we provide a formal statement of 
the general problem we are considering, though the exam- 
ple in this article is less general. Background regarding the 
numerical solution of polynomial systems may be found in 
Section 3. Details about our method are described in Sec- 
tion 4, while Section 5 contains an example. 


2. Formal Problem Statement 


Given two robot geometries, we seek to optimize the place- 
ment of grasping sockets to maximize the post-failure 
workspace (we assume a socket attachment mechanism of 
the first arm to the second as considered in [22]). That is, 
if one robot has a free swinging joint failure, described as 
in [23, 24], we would like to ensure that when the func- 
tional robot joins at the socket, the resulting cooperative 
workspace is as large as possible. The geometry of the 
problem is illustrated at Figure 1. Let the separation dis- 
tance be 6, and without loss of generality, let this transla- 
tion between bases be entirely along the x-axis. Also let the 
point at which a socket is attached to the left robot, here- 
after robot 1, be point P, with the origin at the base of robot 
1. Finally, let the right robot be robot 2. 

We use the Denavit-Hartenberg convention to de- 
scribe the configurations of the two robots. Then we can 
describe the contact point P in terms of the DH parameters 
for robot 1, as P is some distance a from the origin of the 
second frame. A point Q in space can then be said to be in 
the post-failure workspace of robot 1 for a parameter pair 
(ô, a), if there exists a set of joint angles such that end ef- 
fector of robot 1 is at Q, and the end effector of robot 2 is at 
P. Consider two identical planar rotary joint robots offset 
along the x-axis by distance 6, with each joint length equal. 
Then, in Figure 1, we have L! = L? = L} = L2 = L. Fur- 
thermore, normalize link lengths to L = 1. Then the DH 
parameter table for each robot 2 is identical, and equal to: 


In our case, the post-failure workspace, W is a subset R?; 
if we are considering orientation at each point in this space, 
we must consider the direct product of SO(2), the set of 
rotations in two-space, and R?. However, we neglect the 
orientation at the end effector of either robot, and consider 
only the Euclidian measure on R2, so it makes sense to talk 
about the size of Wy. One obvious way to maximize |W} | 
is to set ô = 0 so the two robots have exactly the same base 
point, and make the two robots identical. However, this is 
an impractical situation as first of all, there would be little 
reason to have two robots to begin with, and second, 6 is 
usually set by the original design of the system. Thus, we 
will consider 6 to be a fixed parameter of the problem, but 
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Figure 1. Two robots touching at point P. This is a simplified 

2D model of a more general 3D case. In this paper we will not 

consider the final arm orientation so the workspace is a subset of 
R?. 


examine a set of different 6’s to show the dependence of 
the problem on that parameter. Indeed, each robot has its 
own independent workspace, W1, W2, and when the two 
robots are placed near enough to each other, there is an 
intersection workspace Wn = Wi N Wə. It is clear that 
|W,| depends on ô. 

There are many possible objective functions one can 
imagine, and the right choice depends on the application. 
In this paper, our objective function to be maximized is a 
weighted sum of |W |, |W1], and |W,|: 


Q = (1 — A) |W] + AW] — |Wal)- 


For a given 6, we seek to maximize Q in terms of a, which 
controls the placement of the joining socket. 


3. Homotopy Continuation 


Because the inverse kinematics problems of the next sec- 
tion can be realized as polynomial systems, our approach is 
to use the well-developed methods of numerical algebraic 
geometry, particularly homotopy continuation, to estimate 
the solutions of these polynomial systems. Given a set of 
polynomials f;,..., fn in N variables for which we seek 
the solutions, homotopy methods begin by producing and 
solving some other (related) polynomial system g1,...,9N 
for which the solutions are easily found, e.g., polynomials 
such as zê — 1. By varying the coefficients of g to those 
of f, mathematical theory guarantees that, with probabil- 
ity one, the solutions will vary continuously, thus forming 
solutions curves or paths from the solutions of g to those 
of f. These solution curves may then be tracked numeri- 
cally with standard predictor/corrector methods, such as a 
combination of RK45 and Newton’s method. This is a very 


simple, high-level description of these methods, necessi- 
tated by the brevity of this article. Further details may be 
found in [25, 18]. 

There is one particular setting in which homotopy 
methods are particularly effective, of which we make use 
in this paper. If instead of solving one polynomial system 
f we wish to solve a large number of polynomial systems 
that differ only in their coefficients (i.e., we wish to solve 
f(p), where p is some set of parameters for which we will 
choose a large number of instances), there is an especially 
efficient homotopy method known as a parameter homo- 
topy. When constructing a starting system g as in the pre- 
vious paragraph, it is possible that there will be many more 
solutions of g than f. As a result, it may occur that hun- 
dreds of thousands of solution curves are tracked in order 
to find only a few solutions. However, in the special case 
of a parameter homotopy, we first solve f(p) at a single 
instance of p, say p* (typically chosen as random complex 
numbers, for theoretical reasons). This stage may involve 
a number of wasted paths (as above). However, all other 
instances of f (p) may then be solved by simply following 
the handful of finite solutions from p* to any other choice 
of p. In other words, for an initial parameter choice, we 
may need to follow thousands of paths to find the six solu- 
tions of interest. Then, for all other points in the parameter 
space, it suffices to follow just six paths. Again, there are 
many theorems and details underlying these methods, most 
of which may be found in [18]. 

For this paper, it is enough to accept homotopy con- 
tinuation as a numerical method that will provide approx- 
imations to all isolated solutions of a polynomial system. 
Several software packages are available for these sorts of 
computations. We use a freely-available software package 
named Bertini [26]. 


4. Workspace Computation 
4.1 W; Computation 


We compute the workspaces for each robot via a random 
sampling method. We sample a subset of R? that is guar- 
anteed to contain the workspace using a uniform pseudo- 
random distribution with the built-in MATLAB random 
number generator. We then solve the inverse kinematics 
problem at each of these sample points. The inverse kine- 
matics problem is first solved via a standard homotopy run 
at p*, a random point in complex parameter space. All 
subsequent runs at parameter points in the workspace are 
treated as parameter homotopies, and all such runs begin at 
p = p*. Because Bertini does not distinguish between real 
and complex solutions to a polynomial system, the points 
that resulted in real inverse kinematic solutions are inside 
the workspace, and points that gave only complex solutions 
are outside the workspace. If we have n samplings, and m 
of these have real solutions, we can estimate the (Euclidian) 
measure of the workspace as the product of the volume of 
the sampled box, and the ratio m/n. For example, for the 
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robots under consideration here, a reasonable set of bounds 
for sampling are P = (P,, Py) € [—2,2] x [—2,2]. Then 
if we sampled R? n = 1500 times, and only m = 1174 of 
these points had at least one real solution, the estimated size 
of the workspace would be |W;| = 4 x 4 x 1174/1500 = 
12.5. 

The inverse kinematics equations for a robot with ro- 
tary joints are trigonometric in nature. In order to use ho- 
motopy continuation, we write the trigonometric equations 
in a polynomial form by treating each sine and cosine as a 
separate variable, mapping cos(0;) = c; and sin(@;) = si. 
Taking L = 1, we write the inverse kinematics equations 


as: 
o={ 


These equations are augmented by trigonometric identities, 


(2) 


Four total equations (1,2), in four unknowns, constitute the 
polynomial system we will solve using the homotopy con- 
tinuation solver Bertini. We treat P as a parameter. 

As described above, the solve is performed in two 
steps: first, an initial solve for random complex values of 
P,, Py; second, a solve for each of the n samples, with 
(P,, Py) as the sample itself. The advantage of using a 
parameter homotopy in this case is a reduction of compu- 
tation time by a factor of four: the initial solve tracks eight 
paths to find two complex solutions, and the second solve 
tracks two paths to two solutions. In other words, without 
using parameter homotopies, each solve would require the 
tracking of eight paths rather than two. 

Note that for the case of N > 3 joints working in 
3 dimensions without orientation, there are 6 inverse kine- 
matic equations in 2N variables when computing W;. As 
long as the number of joints equals the number of degrees 
of freedom in the ambient workspace, Bertini will find all 
solutions, and we will be able to measure |W;|. For kine- 
matically redundant robots, W; is a set of higher 2N — 6 
dimensional manifolds, which could be described by defin- 
ing a mesh of the same dimensionality as coordinates on 
the joints. Our method readily applies to that higher dimen- 
sional problem as well. However, the issues we are facing 
are in the storage and data analysis, not in the solution find- 
ing, which is fast. For example, with N = 5 joints, and a 
3 dimension workspace, each point in the workspace is a 
two dimensional manifold. We then have to optimize the 
workspace with respect to contact point on three joints. So 
considering ten points per link, and ten points per dimen- 
sion of manifold W; (which is very crude), the number of 
results we have to analyze is 10°. The work load increases 
dramatically with the growth of dimensionality for both the 
robot and ambient workspace. Another problem is whether 
to use random spatial sampling or a mesh, and how to op- 
timize the number of samples. The optimal sampling will 
depend on the geometry of the manifolds, and has to be 
studied in more detail. 


C102 — $182 + C1 — Py 
S1C2 + C182 + S1 — Pj 


(1) 


e+s?-1=0, i=1,2. 


In any case, the problems we are facing regarding ex- 
tending this method to higher dimensions do not come from 
the method itself, but from sampling and data analysis is- 
sues. Thus, while our method is general and rather power- 
ful, more work will be needed to apply it to complex con- 
figurations. 


4.2 Wn Computation 


A similar computation is performed to find an estimate of 
|W,|. Because we know that Wn C W2, we simply solve 
the inverse kinematics of robot 1 at the m points that gave 
real solutions for W2. Suppose that of n initial samples, m 
gave real solutions for W2, and k correspond to a real solu- 
tion belonging to Wj, then we can estimate the intersection 
work space to have the measure 


k 
[Wa] = — |W]. 
m 


The equations, method, and extensions to higher dimension 
for this step of the program are identical to those in Section 
(4.1), so we will not present them here for brevity. 


4.3 W; Computation 


The final component for calculating the objective function 
is to estimate the post-failure space |W;|. We know that 
Wy C Wi, so as for Wn, we need only consider the m 
points we computed in W1. The major difference in this 
step are the equations. Let i € {1, 2,3, 4}, so that we have 
a sin-cosine pair for each of the two joints in the two robots. 
We want to place the robots such that robot 2 touches robot 
1 at point P, so for a given value of parameter a, we have 
a secondary DH table for robot 1: 


Here, we define c; and s; to be the cosine and since, respec- 
tively, with ¿ = 1, 2 corresponding to the first (failed) robot 
and 7 = 3, 4 corresponding to the second (assisting) robot. 
In this notation, our equations for this step are, 


C1C2 — $182 + C1 — Py 
S1C2 + C1S2 + S1 — Py 


0O=¢ acC — a8, 82 + Cy — (€3C4 — $384 + C3 + Ô) 
as C2 + ac $2 + 81 — (s3c4 + 384 + 53) 
s +c- 1 


We consider a, P}, Py to be parameters for Bertini. We 
again solve these systems in two steps, as before. Us- 
ing the two-step procedure described above, the initial ran- 
dom solve involves 16 paths, and the subsequent parameter 
solves involve only 4 paths. 

Again, we can extend this portion of the program to a 
greater number of dimensions than 2, with 4 equations for 
each dimension of the workspace, encountering the same 
data analysis difficulties as above. 
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Figure 2. Demonstration of grasping configuration. In this ran- 
domly chosen example, a = 0.381 for ô = 0.133. 


5. Example 


The maximum resulting workspace with respect to varying 
a for a given ô determines the optimal placement of, for 
example, a grasping location on robot one, to ensure the 
largest restored workspace in the event of a free-swinging 
failure of the second joint of robot one. 

To demonstrate the method, we show results for a ran- 
dom sampling of n = 10? points for contour plots, and 
n = 10° for Wy plots. The robots are identical, both 
having two joints, all normalized to length one, as in Fig- 
ure (1). For preparation, we sample R?, uniformly dis- 
tributed, using the built in MATLAB random number gen- 
erator. We sample over a rectangle which surely contains 
the workspace of robot two. In this example, we sampled 
over [—2, 2] x [—2, 2]. For each point in the sampling, 
we determine whether it lies in the desired space, by the 
above described method of homotopy continuation, using 
the inverse kinematic equations for the Denavit-Hartenberg 
parameters. 

Because Bertini solves over the complex numbers for 
each variable, we find solutions for each point, regardless 
of whether it lies inside the workspace. However, the points 
which have solutions for which all variables are real-valued 
(numerically real-valued, calling any imaginary part less 
than 1074 zero), are those lying within the workspace. The 
first step in the computation is to compute the workspace 
for both robots, as described in Sec. 4.1. Because the robots 
are identical in this example, there is only one workspace 
computation. The second step in computation is the inter- 
section of the two workspaces, for a specified set of ô val- 
ues, as in Sec. 4.2. Finally, for each pair of values 6 and a 
for which we estimate the post-failure grasping workspace, 
we solve a set of equations as described in Sec. 4.3. For 


Table 1. 


Estimates on the normalized size of post-failure 
workspaces, with robot 2 grasping robot 1. 


5 a Wal em 
0 | Va€ (0, I] 1 I 
0.1 0.98 
1 0.5 0.68 0.86 
0.9 0.72 
0.1 0.64 
2 0.5 0.39 0.54 
0.9 0.42 
0.1 0.07 
3 0.5 0.14 0.14 
0.9 0.15 
>4 | Vae (0, 1] 0 0 
(a) [Wa] (ô). (b) |Wy|(, a). 


Figure 4. (a): The curve of |W,| in depending on 6 is mono- 

tonically decreasing, with some wavering due to low number of 

points in the sample. (b): |W p| depending on (6, a). For small 6, 

robot 2 can grasp robot | at nearly any point P. As 6 increases, a 

becomes more important. For 6 near 4, the distance above which 

the robots cannot contact each other, there is a small range of a 
values that give nonempty W,. 


this example, we computed W} for the set of ô and a, with 
|W | normalized with respect to |W1 |. 

Using a sampling of n = 1000 points, we computed 
both |W,| and |W;|. The size of the intersection workspace 
appears from this coarse approximation to be monotoni- 
cally decreasing as we increase the separation distance, and 
in fact in this case we know it is, because it is merely the in- 
tersection of two circles. See Figure 4(a) and column three 
of Table 5.. 

The post-failure grasping workspace is more interest- 
ing, and depends both on 6 and a. For close placements 
of robot bases, robot 2 can grasp robot 1 at almost any 
spot on the second link; W; is almost identical to W1. As 
the robots move further apart, 6 increases and the number 
of points robot 1 can reach while being grasped at P de- 
creases. Not surprisingly, |W,| is greatest, for ô € [0, 2] 
and a ~ 0, see Figure 4(b). Thus, we should place a ball 
joint near the base of joint 2 to ensure largest Wf for closer 
robot bases. 
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(b) À = 0.333. 


(c) \ = 0.666. 


(d) \=1. 


Figure 5. Objective function contours for a grid of (ô, a) pairs. 


As the separation increases so that the workspaces 
barely overlap, robot 2 may only grasp robot 1 near the 
end effector, and the resulting W; is small. Also, the com- 
puted |W;| has small variation, even though the shape of 
the workspace is quite different. See Figure 3, and column 
four of Table 5.. In these plots, the black area is inaccesi- 
ble, light gray is fully accessible, and dark gray is reachable 
from a limited number of configurations. 

In particular, among the nine workspaces in Figure 3, 
the one that is clearly largest is for (6 , a) = (1, 0.1), repre- 
senting 98% of the original workspace. Among the three 
workspaces for 6 = 3, however, that corresponding to 
a = 0.1 is much smaller; for a = 0.5 and a = 0.9 the 
two spaces have almost identical numerical size, with the 
shape being different. 

Finally, when we weight |W,| with |W] in our ob- 
jective function Q, we can determine an optimal ball joint 
placement. See Figure 5. For weighting factor \ = 0, Q is 
simply the value of |W;|, and the maximizer for any given 
dis a = 0. With À = 1/3, the optimal joint location is still 
at the base of the second link, but occurs with separation 
ô = 1. Increasing À further makes the size of the intersec- 
tion workspace overpower the post-failure workspace, and 
by à = 2/3 the maximum of Q is invariant with respect to 
a. Therefore, the weighting factor, and in fact the choice of 
Q itself, is important. 


6. Conclusion 


We used homotopy continuation, as implemented in 
Bertini, to estimate the size of workspaces, the intersec- 
tion of workspaces, and post-failure grasping workspaces 


in the case of having two two-link serial robots placed near 
one another. The optimal configuration for two two joint 
rotary robots depends on user preference, one example of 
which was presented. A general algorithm for solving the 
problem of finding optimal placement and configuration of 
two such robots was presented. 

The work carried out so far has not been compre- 
hensive. For example, we have not taken into account 
different designs for the two robots (including unequal 
link lengths) or effects of gravity in the case the two- 
dimensional workspaces are not orthogonal to the gravity 
field. However, we anticipate that the general algorithm 
presented in this article can be expanded with little diffi- 
culty to include such considerations. Choices in the algo- 
rithm, such as the use of homotopy continuation, have been 
made to make the generalization of this method to higher- 
dimensional workspaces feasible. These sorts of general- 
izations are presently being considered by the authors. 

It should be noted that the methods of numerical alge- 
braic geometry may be used to compute complex positive- 
dimensional components of the solutions sets of polyno- 
mial systems. However, it is nearly impossible to detect 
positive-dimensional real solutions, particularly above di- 
mension one. Therefore, kinematically redundant robots 
present difficulty for our method. Perhaps we could sys- 
tematically reduce the underconstrained inverse kinematic 
system for such a robot to a fully constrained system; how- 
ever, the curse of dimensionality will likely be problematic. 
This is another direction of ongoing research. 
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(c) (6, a) = (1, 0.9). 


(a) (6, a) = (1, 0.1). 


(e) (6, a) = (2, 0.5). 
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® (8, a) = (3, 0.1). (h) (6, a) = (3, 0.5). © (6, a) = (3, 0.9). 


Figure 3. Example W+. Black points are not reachable in the post-failure space. Dark gray areas are reachable in two configurations, as one 
for robot 1, and two for robot 2. Light gray areas are reachable in four configurations, as two per robot. 
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